#include "one_camera_tracker.h"
#include "math/segment2d.h"

#include "json.hpp"

#include <fstream>

using namespace perception::camera;

static inline void split(const std::string& str, std::vector<std::string>& result, const std::string& delim = " ",
    bool insertEmpty = false)
{
    if (str.empty() || delim.empty()) {
        return;
    }

    std::string::const_iterator substart = str.begin();
    std::string::const_iterator subend = substart;
    while (true) {
        subend = std::search(substart, str.end(), delim.begin(), delim.end());
        std::string temp(substart, subend);

        if (!temp.empty()) {
            result.push_back(temp);
        } else if (insertEmpty) {
            result.push_back("");
        }

        if (subend == str.end())
            break;
        substart = subend + delim.size();
    }
}

OneCameraTracker::OneCameraTracker(std::string inex, int pw, int ph, int rw, int rh, std::string roi, std::string camera)
    : inex_(std::move(inex))
    , p_image_width_(pw)
    , p_image_height_(ph)
    , r_image_width_(rw)
    , r_image_height_(rh)
    , roi_file_(std::move(roi))
    , camera_(std::move(camera))
    , logger_(rclcpp::get_logger("OneCameraTracker"))
{
    const double track_match_thresh = 10;
    const int track_max_time_lost = 0; // 15;
    tracker_ = std::shared_ptr<BYTETracker>(new BYTETracker);
    tracker_->init(track_match_thresh, track_max_time_lost);

    std::fstream inexfile(inex_);
    std::stringstream ss;
    ss << inexfile.rdbuf();
    auto inex_context_ = ss.str();

    std::cout << "read inex from " << inex_ << std::endl;
    nlohmann::json j = nlohmann::json::parse(inex_context_);

    camera_matrix_ = cv::Matx<double, 3, 3>(
        j["K"][0][0], j["K"][0][1], j["K"][0][2],
        j["K"][1][0], j["K"][1][1], j["K"][1][2],
        j["K"][2][0], j["K"][2][1], j["K"][2][2]);
    std::cout << "camera_matrix_: " << camera_matrix_ << std::endl;

    dist_coeffs_ = cv::Matx<double, 1, 5>(j["distort"][0], j["distort"][1], j["distort"][2], j["distort"][3], j["distort"][4]);
    std::cout << "dist_coeffs_: " << dist_coeffs_ << std::endl;

    cam2ground_ = cv::Matx<double, 3, 3>(
        j["cam2ground"][0][0], j["cam2ground"][0][1], j["cam2ground"][0][2],
        j["cam2ground"][1][0], j["cam2ground"][1][1], j["cam2ground"][1][2],
        j["cam2ground"][2][0], j["cam2ground"][2][1], j["cam2ground"][2][2]);
    std::cout << "cam2ground_:  " << cam2ground_ << std::endl;

    // read 3d roi
    // read_3d_roi();
    read_2d_roi();
}

void OneCameraTracker::process(const std::vector<ObjectDetectInfoPtr>& detected_objects, std::vector<ObjectTrackInfoPtr>& tracked_objects,
    const cv::Mat& img, int64_t ts_ms)
{
    std::vector<ObjectDetectInfoPtr> objects;
    // 2d roi
    if (valid_roi_2d_) {
        for (auto iter = detected_objects.begin(); iter != detected_objects.end(); iter++) {
            if (filter_out(roi_2d_, (*iter)->box.left_top.x, (*iter)->box.left_top.y, (*iter)->box.Width(), (*iter)->box.Height())) {
                RCLCPP_INFO(logger_, "ROI2D %s out %0.1f %0.1f %0.1f %0.1f", camera_.c_str(), (*iter)->box.left_top.x, (*iter)->box.left_top.y, (*iter)->box.Width(), (*iter)->box.Height());
                continue;
            }
            RCLCPP_INFO(logger_, "ROI2D %s notout %0.1f %0.1f %0.1f %0.1f", camera_.c_str(), (*iter)->box.left_top.x, (*iter)->box.left_top.y, (*iter)->box.Width(), (*iter)->box.Height());
            objects.push_back(*iter);
        }
    }

    tracker_->update(objects, tracked_objects, img, ts_ms);

    for (auto iter = tracked_objects.begin(); iter != tracked_objects.end();) {
        auto& tracked = *iter;
        tracked->tracker.track_id = std::fabs(tracked->tracker.track_id);

        convert_uv2xy("bottom uv", tracked->detect.bottom_uv.x, tracked->detect.bottom_uv.y, tracked->tracker.x, tracked->tracker.y);

        {
            double p1x = 0, p1y = 0, p2x = 0, p2y = 0;

            convert_uv2xy("pts8[3]", tracked->detect.pts8.pts8[3].x, tracked->detect.pts8.pts8[3].y, p1x, p1y);
            convert_uv2xy("pts8[2]", tracked->detect.pts8.pts8[2].x, tracked->detect.pts8.pts8[2].y, p2x, p2y);

            decision::math::Segment2d segment(decision::math::Vec2d(p1x, p1y), decision::math::Vec2d(p2x, p2y));
            tracked->tracker.width = segment.length();
        }

        {
            double p1x = 0, p1y = 0, p2x = 0, p2y = 0;

            convert_uv2xy("pts8[0-1]", (tracked->detect.pts8.pts8[0].x + tracked->detect.pts8.pts8[1].x) / 2.0,
                (tracked->detect.pts8.pts8[0].y + tracked->detect.pts8.pts8[1].y) / 2.0, p1x, p1y);
            convert_uv2xy("pts8[3-2]", (tracked->detect.pts8.pts8[3].x + tracked->detect.pts8.pts8[2].x) / 2.0,
                (tracked->detect.pts8.pts8[3].y + tracked->detect.pts8.pts8[2].y) / 2.0, p2x, p2y);

            decision::math::Segment2d segment(decision::math::Vec2d(p2x, p2y), decision::math::Vec2d(p1x, p1y));
            tracked->tracker.length = segment.length();
            tracked->tracker.hea = legalize_angle((M_PI_2 - segment.heading()) / M_PI * 180);
        }

        {
            speed_calcer_[tracked->tracker.track_id].add_pos(ts_ms, tracked->tracker.x, tracked->tracker.y);
            speed_calcer_[tracked->tracker.track_id].speed(ts_ms, tracked->tracker.speedx, tracked->tracker.speedy);
            // speed_calcer_[tracked->tracker.track_id].stable_position(ts_ms, tracked->tracker.x, tracked->tracker.y);

            speed_calcer_[tracked->tracker.track_id].add_heading(tracked->tracker.hea);

            // RCLCPP_INFO(logger_, "SPEED %f %f", tracked->tracker.speedx, tracked->tracker.speedy);
            // if ((fabs(tracked->tracker.speedx) < 1 && fabs(tracked->tracker.speedy) < 1)  
            //     || !isnan(tracked->tracker.speedx) 
            //     || !isnan(tracked->tracker.speedy)) {
            //     RCLCPP_INFO(logger_, "SPEED old style" );
            //     tracked->tracker.hea = speed_calcer_[tracked->tracker.track_id].heading();
            // } else {
            //     RCLCPP_INFO(logger_, "SPEED new style" );
            const auto hea = speed_calcer_[tracked->tracker.track_id].heading();
                // tracked->tracker.hea = std::isnan(hea) ? speed_calcer_[tracked->tracker.track_id].heading() : hea;
            // }

        }

        RCLCPP_INFO(logger_, "inex %s. tid:%d (%.2f %.2f)->(%.2f %.2f %.2f)", inex_.c_str(), tracked->tracker.track_id,
            tracked->detect.bottom_uv.x, tracked->detect.bottom_uv.y, tracked->tracker.x, tracked->tracker.y, tracked->tracker.hea);

        // 3d roi
        decision::math::Vec2d point(tracked->tracker.x, tracked->tracker.y);
        if (valid_roi_3d_ && !roi_3d_.is_point_in(point)) {
            iter = tracked_objects.erase(iter);
        } else {
            iter++;
        }
    }

    for (auto iter = speed_calcer_.begin(); iter != speed_calcer_.end();) {
        if (ts_ms - iter->second.updated_time_ms_ > 20 * 1000) {
            iter = speed_calcer_.erase(iter);
        } else {
            iter++;
        }
    }
}

void OneCameraTracker::convert_uv2xy(const std::string& hit, const double u, const double v, double& x, double& y)
{
    RCLCPP_INFO(logger_, "%s,  %.2f, %d, %d ; %.2f,%d,%d", hit.c_str(),
        u, r_image_width_, p_image_width_, v, r_image_height_, p_image_height_);
    std::vector<cv::Point2f> input = { cv::Point(u / r_image_width_ * p_image_width_, v / r_image_height_ * p_image_height_) };
    std::vector<cv::Point2f> undistorted;
    std::vector<cv::Point2f> wxy;

    cv::undistortPoints(input, undistorted, camera_matrix_, dist_coeffs_);
    cv::perspectiveTransform(undistorted, wxy, cam2ground_);

    x = wxy[0].y * -1;
    y = wxy[0].x;
    RCLCPP_INFO(logger_, "   %s, %s coord conv uv(%.2f,%.2f) undist(%.2f,%.2f) ground(%.2f,%.2f)->(%.2f,%.2f)",
        inex_.c_str(),
        hit.c_str(),
        input[0].x, input[0].y,
        undistorted[0].x, undistorted[0].y,
        wxy[0].x, wxy[0].y,
        x, y);
}

bool OneCameraTracker::filter_out(const cv::Mat& roi, int x, int y, int w, int h)
{
    if (roi.empty()) {
        return false;
    }

    return roi.at<uchar>(y, x) < 127 && roi.at<uchar>(y + h, x) < 127
        && roi.at<uchar>(y + h, x + w) < 127 && roi.at<uchar>(y, x + w) < 127;
}

void OneCameraTracker::read_3d_roi()
{
    if (roi_file_.empty()) {
        valid_roi_3d_ = false;
        return;
    }

    std::fstream roifile(roi_file_);
    std::stringstream ss;
    ss << roifile.rdbuf();
    auto roi_context_ = ss.str();

    std::vector<decision::math::Vec2d> vecs;
    std::vector<std::string> points;
    split(roi_context_, points, ",");

    valid_roi_3d_ = (!points.empty() && 0 == points.size() % 2);
    if (valid_roi_3d_) {
        for (size_t i = 0; i < points.size(); i += 2) {
            vecs.push_back(decision::math::Vec2d(atof(points[i].c_str()), atof(points[i + 1].c_str())));
        }
        roi_3d_ = decision::math::Polygon2d(vecs);
    }
}

void OneCameraTracker::read_2d_roi()
{
    if (roi_file_.empty()) {
        valid_roi_2d_ = false;
        return;
    }
    try {
        cv::resize(cv::imread(roi_file_), roi_2d_, cv::Size(r_image_width_, r_image_height_));
        std::vector<cv::Mat> mats(3);
        cv::split(roi_2d_, mats);
        roi_2d_ = mats[0];
    }
    catch(...) {
        valid_roi_2d_ = false;
        return;
    }

    valid_roi_2d_ = true;
}
